#include "globals.h"
#include "matrix.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

// Changed matrix_new and matrix_col_resize to allow the existence of matrices with zero columns. -JB
struct matrix *matrix_new(int cols, int rows){
	struct matrix *o;
	o = malloc(sizeof(struct matrix));
	o->ar = rows;
	o->ac = cols;
	o->nc = 0;
	if(cols > 0) { ///
 		o->m = malloc(cols * sizeof(matrix_t **));
		matrix_t *tmp;
		tmp = malloc(rows * cols * sizeof(matrix_t));
		int i;
		for(i = 0; i < cols; i++){
			o->m[i] = tmp + (i * rows * sizeof(matrix_t));
		}
	} ///
	return o;
}

void matrix_col_resize(struct matrix *m, int cols){
  
	///
	if (m->ac == 0) {
		int rows = m->ar;
		m->ac = cols;
		m->m = malloc(cols * sizeof(matrix_t **));
		matrix_t *tmp;
		tmp = malloc(rows * cols * sizeof(matrix_t));
		int i;
		for(i = 0; i < cols; i++){
			m->m[i] = tmp + (i * rows * sizeof(matrix_t));
		}
		return;
	}
	///
  
	int oldcols;
	//oldcols = m->ac;
	m->ac = cols;
	matrix_t *tmp;
	debug_w("matrix_col_resize: new array size: %x\n", m->ar * cols * sizeof(matrix_t));
	tmp = m->m[0] = realloc(m->m[0], m->ar * cols * sizeof(matrix_t));
	m->m = realloc(m->m, cols * sizeof(matrix_t *));
	debug_w("matrix_col_resize: new array start: %x\n", (unsigned int)  tmp);
	for(oldcols = 1; oldcols < cols; oldcols++){
		//fprintf(stderr, "matrix_col_resize: pointer increment: %x\n", m->ar * sizeof(matrix_t));
		m->m[oldcols] = tmp + (oldcols * m->ar/* * sizeof(matrix_t)*/);
	}
	if(m->nc > m->ac){
		m->nc = m->ac;
	}
}

void matrix_resize(struct matrix *m, int cols, int rows){
	m->ar = rows;
	m->ac = cols;
	matrix_t *tmp;
	tmp = m->m[0] = realloc(m->m[0], rows * cols * sizeof(matrix_t **));
	m->m = realloc(m->m, cols * sizeof(matrix_t *));
	int i;
	for(i = 1; i < cols; i++){
		m->m[i] = tmp + (i * rows/* apparently redundant: * sizeof(matrix_t) */);
	}
}

int matrix_addPoint(struct matrix *m, matrix_t *point) {
	matrix_print(m);
	if(m->ac <= m->nc){
		matrix_col_resize(m, m->ac + 1);
	}
	m->m[m->nc][0] = point[0];
	m->m[m->nc][1] = point[1];
	m->m[m->nc][2] = point[2];
	m->m[m->nc][3] = 1;
	matrix_print(m);
	return m->nc++;
}

int matrix_concatenate(struct matrix *a, struct matrix *b) {
  matrix_col_resize(a, a->ac + b->ac);
  int i;
  for(i = 0; i < b->ac; i++) {
    matrix_addPoint(a, b->m[i]);
    a->nc++;
  }
  return a->nc;
}

void matrix_free(struct matrix *m){
	free(m->m[0]);
	free(m->m);
	free(m);
}

void matrix_print(struct matrix *m){
	int i, j;
	printf("[\n  ar: %d, ac: %d, nc: %d.\n  [ ", m->ar, m->ac, m->nc);
	for(i = 0; i < m->ac; i++){
		printf("%11x ", (unsigned int) m->m[i]);
	}
	printf("]\n");
	for(i = 0; i < m->ar; i++){
		printf("  [ ");
		for(j = 0; j < m->ac; j++){
			printf("%11lf ", m->m[j][i]);
		}
		printf("]\n");
	}
	printf("]\n");
}

void matrix_ident(struct matrix *m){
	int i, j;
	for(i = 0; i < m->ac; i++){
		for(j = 0; j < m->ar; j++){
			if(i == j){
				m->m[i][j] = 1;
			} else {
				m->m[i][j] = 0;
			}
		}
	}
}

void matrix_scalar_mult(struct matrix *m, matrix_t multiple){
	int i, j;
	for(i = 0; i < m->ac; i++){
		for(j = 0; j < m->ar; j++){
			m->m[i][j] *= multiple;
		}
	}
}

void matrix_mult(struct matrix *a, struct matrix *b, struct matrix *o){
	int i, j, k;
	matrix_t tmp;
	matrix_t **ma, **mb, **mo;
	ma = a->m;
	mb = b->m;
	mo = o->m;
	for(i = 0; i < a->ar; i++){
		for(j = 0; j < b->ac; j++){
			tmp = ma[0][i] * ma[j][0];
			for(k = 1; k < a->ac; k++){
				tmp += ma[k][i] * ma[j][k];
			}
			mo[i][j] = tmp;
		}
	}			
}

void matrix_mult_inplace(struct matrix *a, struct matrix *b){
	int i, j, k;
	int orows = b->ar;
	matrix_t mt[a->ar * b->ar];
	matrix_t tmp;
	matrix_t **ma, **mb;
	ma = a->m;
	mb = b->m;
	for(i = 0; i < a->ar; i++){
		for(j = 0; j < b->ac; j++){
			tmp = ma[0][i] * ma[j][0];
			for(k = 1; k < a->ac; k++){
				tmp += ma[k][i] * ma[j][k];
			}
			mt[j * orows + i] = tmp;
		}
	}
	memcpy(mt, *mb, a->ar * b->ar * sizeof(matrix_t));
}

void matrix_copy(struct matrix *i, struct matrix *o){
	if(i->ar != o->ar || i->ac != o->ac){
		matrix_resize(o, i->ac, i->ar);
	}
	memcpy(i->m[0], o->m[0], i->ar * i->ac * sizeof(matrix_t));
}
